//============================================================================
// Name        : test.cpp
// Author      : Fabien Bonardi
// Version     :
// Copyright   : Your copyright notice
// Description : Hello World in C++, Ansi-style
//============================================================================

#include <iostream>
using namespace std;

void degreeToRadian(float *angleDeg);
void MultiplicationMatrices(float * A, float * B, float * R);
void AdditionMatrices(float * A, float * B, float * R);
void MultiplicationMatriceVecteur(float * A, float * V, float * R);
void MultiplicationCoeffMatrice(float * a, float * A, float * R);
float NormeVecteur(float * A);
void AntisymV(float * V, float * R);

float matrice[3][3];
float matriceA[3][3];
float matriceB[3][3];
float vecteur[3];
float vecteura[3];
float a = 32;

int main() {

	vecteur[0] = 1;
	vecteur[1] = 2;
	vecteur[2] = 3;

	matrice[0][0] = 1;
	matrice[0][1] = 2;
	matrice[0][2] = 3;
	matrice[1][0] = 4;
	matrice[1][1] = 5;
	matrice[1][2] = 6;
	matrice[2][0] = 7;
	matrice[2][1] = 8;
	matrice[2][2] = 9;

	matriceA[0][0] = 1;
	matriceA[0][1] = 1;
	matriceA[0][2] = 1;
	matriceA[1][0] = 1;
	matriceA[1][1] = 1;
	matriceA[1][2] = 1;
	matriceA[2][0] = 1;
	matriceA[2][1] = 1;
	matriceA[2][2] = 1;

	cout << "www.fabienbonardi.fr" << endl; // prints www.fabienbonardi.fr

	cout << vecteur[0] << "  " << vecteur[1] << "  " << vecteur[2] << "  "
			<< endl;
	cout << endl;

	cout << matriceA[0][0] << "  " << matriceA[0][1] << "  " << matriceA[0][2]
			<< "  " << endl;
	cout << matriceA[1][0] << "  " << matriceA[1][1] << "  " << matriceA[1][2]
			<< "  " << endl;
	cout << matriceA[2][0] << "  " << matriceA[2][1] << "  " << matriceA[2][2]
			<< "  " << endl;

	cout << endl;

	//AntisymV(vecteur, (float *)matrice);

	cout << matrice[0][0] << "  " << matrice[0][1] << "  " << matrice[0][2]
			<< "  " << endl;
	cout << matrice[1][0] << "  " << matrice[1][1] << "  " << matrice[1][2]
			<< "  " << endl;
	cout << matrice[2][0] << "  " << matrice[2][1] << "  " << matrice[2][2]
			<< "  " << endl;

	cout << endl;

	MultiplicationCoeffMatrice(&a, (float *) matriceA, (float *) matriceB);

//	cout << vecteura[0] << "  " << vecteura[1] << "  " << vecteura[2] << "  " << endl;

	cout << matriceB[0][0] << "  " << matriceB[0][1] << "  " << matriceB[0][2]
			<< "  " << endl;
	cout << matriceB[1][0] << "  " << matriceB[1][1] << "  " << matriceB[1][2]
			<< "  " << endl;
	cout << matriceB[2][0] << "  " << matriceB[2][1] << "  " << matriceB[2][2]
			<< "  " << endl;

	cout << NormeVecteur(vecteur);

	return 1;
}

void degreeToRadian(float *angleDeg) {
	*angleDeg *= 3.14159 / 180;
}

void MultiplicationMatrices(float * A, float * B, float * R) {

	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			(&R[i * 3])[j] = (float) 0;
		}
	}

	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			for (int k = 0; k < 3; k++) {
				(&R[i * 3])[j] += ((&A[i * 3])[k] * (&B[k * 3])[j]);
			}
		}
	}
}

void AdditionMatrices(float * A, float * B, float * R) {

	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			(&R[i * 3])[j] = (&A[i * 3])[j] + (&B[i * 3])[j];
		}
	}
}

void MultiplicationMatriceVecteur(float * A, float * V, float * R) {

	for (int j = 0; j < 3; j++) {
		R[j] = 0;
	}

	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			R[i] += (&A[i * 3])[j] * V[j];
		}
	}
}

void MultiplicationCoeffMatrice(float * a, float * A, float * R) {

	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			(&R[i * 3])[j] = *a * (&A[i * 3])[j];
		}
	}
}

float NormeVecteur(float * A) {

	return (A[0] * A[0] + A[1] * A[1] + A[2] * A[2]);
}

void AntisymV(float * V, float * R) {
	(&R[0 * 3])[0] = 0;
	(&R[0 * 3])[1] = -V[2];
	(&R[0 * 3])[2] = V[1];
	(&R[1 * 3])[0] = V[2];
	(&R[1 * 3])[1] = 0;
	(&R[1 * 3])[2] = -V[0];
	(&R[2 * 3])[0] = -V[1];
	(&R[2 * 3])[1] = V[0];
	(&R[2 * 3])[2] = 0;
}
